#include "Circle.h"
#include <Windows.h>
#include <gl\gl.h>  
#define _USE_MATH_DEFINES
#include <math.h>
#include "Line.h"
#include "BodyInfo.h"
#include <iostream>
#include "Game.h"
#include "Rope.h"


#define CATCH_NAN


Circle::Circle(void) :	Primitive(Vector(0,0,0), Vector(0,0,0), 1.0f, 0.1f, true), m_radius(0.8f), support(0)
{
	setElasticity(0.8f);
	setFriction(0.002f);
}

Circle::~Circle(void)
{
}

// Finds the current cell the circle is in
void	Circle::updateCell()
{
	int grid_size = 4;
	
	int x = (int)((m_data->m_pos.GetX() + 20.0f) / grid_size);
	int y = (int)((m_data->m_pos.GetY() + 20.0f) / grid_size);

	m_cell.Set((float)x, (float)y);
}

void	Circle::CalculatePhysics(float dt)
{
	if (!m_data->m_bActive) return;

	Vector force(0.0f, -9.81f * mass());
	Vector accel = force / mass();

	// simple Euler
	m_velocity = m_velocity + accel * dt;
	m_angularVel = m_angularVel + m_angularAccel * dt;
	
	updateCell();
}

bool	Circle::isNaN(Vector v) 
{ 
	if (v.GetX() != v.GetX() || v.GetY() != v.GetY())  {
		return true; 
	}
	else 
		return false; 
}


void	Circle::CollisionWithCircle(Circle* circle, ContactManifold *contactManifold, float dt)
{

	// if both circles are inactive return
	if (getActive() == false && circle->getActive() == false)
		return;

	if (!m_data->m_bActive) return;

	// s = s1.center - s2.center
	Vector s = m_data->m_pos - circle->GetPos();
	// v = v1.velocity - v2.velocity
	Vector v = m_velocity - circle->GetVel();
	// r = s1.radius + s2.radius
	float r = this->m_radius + circle->GetRadius();
	// c = s.s - r^2
	float c = s.dot(s) - r*r;
	float t;
	if (c<0.0f) {
		t = 0.0f;
		return;
	}
	float a = v.dot(v);
	if (a < 0.000001) return;
	float b = v.dot(s);
	if (b >= 0.0f) return;
	float d = b*b - a*c;
	if (d<0.0f) return;
		
	t = (-b-sqrt(d)) / a;
	if (t >= 0.0f && t <= dt)
	{
		ManifoldPoint mpoint;
		mpoint.contactID1 = this;
		mpoint.contactID2 = circle;
		mpoint.contactPoint = m_data->m_pos + m_velocity * t;
		mpoint.responded = false;
		mpoint.impulse = 0.0f;
		mpoint.t_impulse = 0;
		mpoint.contactNormal = Vector(m_data->m_pos - circle->getData()->m_pos).normalise();
		contactManifold->Add(mpoint);
		// wake circles
		this->setActive(true);
		circle->setActive(true);
		return;
	}
	
}

void	updateCell()
{

}


void	Circle::Update(float dt)
{
	if (!m_data->m_bActive) return;

	// Simple Euler
	m_data->m_pos = m_data->m_pos + m_velocity * dt;	
	m_data->m_omega = m_data->m_omega + m_angularVel.GetZ() * dt;

}




float	Circle::GetRadius() const
{
	return m_radius;
}

// 
void	Circle::CollisionResponseWith(ManifoldPoint& point, float dt)
{
	if (!m_data->m_bActive) return;

	point.contactID2->CollisionResponseWithCircle(point, dt);
}


void	Circle::CollisionResponseWithLine(ManifoldPoint& point, float dt)
{

	Circle* c1 = this;
	Line* l = (Line*)point.contactID2;

	////////////////////////////////////////////////////////////////////////////

	//----------------------
	if (l->GetPos(0).GetX() >= -4.0f && l->GetPos(0).GetX() < 3.9f &&
		l->GetPos(0).GetY() <= 12.0f && l->GetPos(0).GetY() >= 0.0f)  {
			
			if (l->getNormal().dot(c1->GetPos()-l->GetPos(0)) > 0.0f && m_game->activeRope())
				m_game->rope().addForce(Vector(0, -m_data->m_mass * 9.81f));
	}

	//----------------------
	CollisionResponse(point, dt);
}

void	Circle::CollisionResponseWithLine2(ManifoldPoint& point, float dt)
{
	
}


void	Circle::CollisionResponseWithCircle(ManifoldPoint& point, float dt)
{
	CollisionResponse(point, dt);
}


Vector	Circle::vectorTo(Vector p)
{
	return  (p - GetPos());
}

Vector&	Circle::cell()
{
	return m_cell;
}